/*
 * This file is part of the OpenMV project.
 *
 * Copyright (c) 2013-2021 Ibrahim Abdelkader <iabdalkader@openmv.io>
 * Copyright (c) 2013-2021 Kwabena W. Agyeman <kwagyeman@openmv.io>
 *
 * This work is licensed under the MIT license, see the file LICENSE for details.
 *
 * MT9M114 driver.
 */
#include "cam_boardconfig.h"
#if (OMV_ENABLE_MT9M114 == 1)

#include <stdint.h>
#include <stdlib.h>
#include <string.h>

#include "omv_i2c.h"
#include "sensor.h"
#include "mt9m114.h"
#include "mt9m114_regs.h"


#define DUMMY_LINES             8
#define DUMMY_COLUMNS           8

#define SENSOR_WIDTH            1296
#define SENSOR_HEIGHT           976

#define ACTIVE_SENSOR_WIDTH     (SENSOR_WIDTH - (2 * DUMMY_COLUMNS))
#define ACTIVE_SENSOR_HEIGHT    (SENSOR_HEIGHT - (2 * DUMMY_LINES))

#define DUMMY_WIDTH_BUFFER      8
#define DUMMY_HEIGHT_BUFFER     8

static int16_t readout_x = 0;
static int16_t readout_y = 0;

static uint16_t readout_w = ACTIVE_SENSOR_WIDTH;
static uint16_t readout_h = ACTIVE_SENSOR_HEIGHT;

static const uint16_t default_regs[][2] = {
    // Sensor Optimization
    {0x316A, 0x8270},
    {0x316C, 0x8270},
    {0x3ED0, 0x2305},
    {0x3ED2, 0x77CF},
    {0x316E, 0x8202},
    {0x3180, 0x87FF},
    {0x30D4, 0x6080},
    {0xA802, 0x0008},

    // Errata 1 (Column Bands)
    {0x3E14, 0xFF39}
};

static const uint8_t patch_0202[] = {
    0xd0, 0x00, 0x70, 0xcf, 0xff, 0xff, 0xc5, 0xd4, 0x90, 0x3a, 0x21, 0x44, 0x0c, 0x00, 0x21, 0x86,
    0x0f, 0xf3, 0xb8, 0x44, 0xb9, 0x48, 0xe0, 0x82, 0x20, 0xcc, 0x80, 0xe2, 0x21, 0xcc, 0x80, 0xa2,
    0x21, 0xcc, 0x80, 0xe2, 0xf4, 0x04, 0xd8, 0x01, 0xf0, 0x03, 0xd8, 0x00, 0x7e, 0xe0, 0xc0, 0xf1,
    0x08, 0xba, 0x06, 0x00, 0xc1, 0xa1, 0x76, 0xcf, 0xff, 0xff, 0xc1, 0x30, 0x6e, 0x04, 0xc0, 0x40,
    0x71, 0xcf, 0xff, 0xff, 0xc7, 0x90, 0x81, 0x03, 0x77, 0xcf, 0xff, 0xff, 0xc7, 0xc0, 0xe0, 0x01,
    0xa1, 0x03, 0xd8, 0x00, 0x0c, 0x6a, 0x04, 0xe0, 0xb8, 0x9e, 0x75, 0x08, 0x8e, 0x1c, 0x08, 0x09,
    0x01, 0x91, 0xd8, 0x01, 0xae, 0x1d, 0xe5, 0x80, 0x20, 0xca, 0x00, 0x22, 0x20, 0xcf, 0x05, 0x22,
    0x0c, 0x5c, 0x04, 0xe2, 0x21, 0xca, 0x00, 0x62, 0xe5, 0x80, 0xd9, 0x01, 0x79, 0xc0, 0xd8, 0x00,
    0x0b, 0xe6, 0x04, 0xe0, 0xb8, 0x9e, 0x70, 0xcf, 0xff, 0xff, 0xc8, 0xd4, 0x90, 0x02, 0x08, 0x57,
    0x02, 0x5e, 0xff, 0xdc, 0xe0, 0x80, 0x25, 0xcc, 0x90, 0x22, 0xf2, 0x25, 0x17, 0x00, 0x10, 0x8a,
    0x73, 0xcf, 0xff, 0x00, 0x31, 0x74, 0x93, 0x07, 0x2a, 0x04, 0x10, 0x3e, 0x93, 0x28, 0x29, 0x42,
    0x71, 0x40, 0x2a, 0x04, 0x10, 0x7e, 0x93, 0x49, 0x29, 0x42, 0x71, 0x41, 0x2a, 0x04, 0x10, 0xbe,
    0x93, 0x4a, 0x29, 0x42, 0x71, 0x4b, 0x2a, 0x04, 0x10, 0xbe, 0x13, 0x0c, 0x01, 0x0a, 0x29, 0x42,
    0x71, 0x42, 0x22, 0x50, 0x13, 0xca, 0x1b, 0x0c, 0x02, 0x84, 0xb3, 0x07, 0xb3, 0x28, 0x1b, 0x12,
    0x02, 0xc4, 0xb3, 0x4a, 0xed, 0x88, 0x71, 0xcf, 0xff, 0x00, 0x31, 0x74, 0x91, 0x06, 0xb8, 0x8f,
    0xb1, 0x06, 0x21, 0x0a, 0x83, 0x40, 0xc0, 0x00, 0x21, 0xca, 0x00, 0x62, 0x20, 0xf0, 0x00, 0x40,
    0x0b, 0x02, 0x03, 0x20, 0xd9, 0x01, 0x07, 0xf1, 0x05, 0xe0, 0xc0, 0xa1, 0x78, 0xe0, 0xc0, 0xf1,
    0x71, 0xcf, 0xff, 0xff, 0xc7, 0xc0, 0xd8, 0x40, 0xa9, 0x00, 0x71, 0xcf, 0xff, 0xff, 0xd0, 0x2c,
    0xd8, 0x1e, 0x0a, 0x5a, 0x04, 0xe0, 0xda, 0x00, 0xd8, 0x00, 0xc0, 0xd1, 0x7e, 0xe0
};
static const uint8_t patch_0302[] = {
    0xd1, 0x2c, 0x70, 0xcf, 0xff, 0xff, 0xc5, 0xd4, 0x90, 0x3a, 0x21, 0x44, 0x0c, 0x00, 0x21, 0x86,
    0x0f, 0xf3, 0xb8, 0x44, 0x26, 0x2f, 0xf0, 0x08, 0xb9, 0x48, 0x21, 0xcc, 0x80, 0x21, 0xd8, 0x01,
    0xf2, 0x03, 0xd8, 0x00, 0x7e, 0xe0, 0xc0, 0xf1, 0x71, 0xcf, 0xff, 0xff, 0xc6, 0x10, 0x91, 0x0e,
    0x20, 0x8c, 0x80, 0x14, 0xf4, 0x18, 0x91, 0x0f, 0x20, 0x8c, 0x80, 0x0f, 0xf4, 0x14, 0x91, 0x16,
    0x20, 0x8c, 0x80, 0x0a, 0xf4, 0x10, 0x91, 0x17, 0x20, 0x8c, 0x88, 0x07, 0xf4, 0x0c, 0x91, 0x18,
    0x20, 0x86, 0x0f, 0xf3, 0xb8, 0x48, 0x08, 0x0d, 0x00, 0x90, 0xff, 0xea, 0xe0, 0x81, 0xd8, 0x01,
    0xf2, 0x03, 0xd8, 0x00, 0xc0, 0xd1, 0x7e, 0xe0, 0x78, 0xe0, 0xc0, 0xf1, 0x71, 0xcf, 0xff, 0xff,
    0xc6, 0x10, 0x91, 0x0e, 0x20, 0x8c, 0x80, 0x0a, 0xf4, 0x18, 0x91, 0x0f, 0x20, 0x8c, 0x88, 0x07,
    0xf4, 0x14, 0x91, 0x16, 0x20, 0x8c, 0x80, 0x0a, 0xf4, 0x10, 0x91, 0x17, 0x20, 0x8c, 0x88, 0x07,
    0xf4, 0x0c, 0x91, 0x18, 0x20, 0x86, 0x0f, 0xf3, 0xb8, 0x48, 0x08, 0x0d, 0x00, 0x90, 0xff, 0xd9,
    0xe0, 0x80, 0xd8, 0x01, 0xf2, 0x03, 0xd8, 0x00, 0xf1, 0xdf, 0x90, 0x40, 0x71, 0xcf, 0xff, 0xff,
    0xc5, 0xd4, 0xb1, 0x5a, 0x90, 0x41, 0x73, 0xcf, 0xff, 0xff, 0xc7, 0xd0, 0xb1, 0x40, 0x90, 0x42,
    0xb1, 0x41, 0x90, 0x43, 0xb1, 0x42, 0x90, 0x44, 0xb1, 0x43, 0x90, 0x45, 0xb1, 0x47, 0x90, 0x46,
    0xb1, 0x48, 0x90, 0x47, 0xb1, 0x4b, 0x90, 0x48, 0xb1, 0x4c, 0x90, 0x49, 0x19, 0x58, 0x00, 0x84,
    0x90, 0x4a, 0x19, 0x5a, 0x00, 0x84, 0x88, 0x56, 0x1b, 0x36, 0x80, 0x82, 0x88, 0x57, 0x1b, 0x37,
    0x80, 0x82, 0x90, 0x4c, 0x19, 0xa7, 0x00, 0x9c, 0x88, 0x1a, 0x7f, 0xe0, 0x1b, 0x54, 0x80, 0x02,
    0x78, 0xe0, 0x71, 0xcf, 0xff, 0xff, 0xc3, 0x50, 0xd8, 0x28, 0xa9, 0x0b, 0x81, 0x00, 0x01, 0xc5,
    0x03, 0x20, 0xd9, 0x00, 0x78, 0xe0, 0x22, 0x0a, 0x1f, 0x80, 0xff, 0xff, 0xd4, 0xe0, 0xc0, 0xf1,
    0x08, 0x11, 0x00, 0x51, 0x22, 0x40, 0x12, 0x00, 0xff, 0xe1, 0xd8, 0x01, 0xf0, 0x06, 0x22, 0x40,
    0x19, 0x00, 0xff, 0xde, 0xd8, 0x02, 0x1a, 0x05, 0x10, 0x02, 0xff, 0xf2, 0xf1, 0x95, 0xc0, 0xf1,
    0x0e, 0x7e, 0x05, 0xc0, 0x75, 0xcf, 0xff, 0xff, 0xc8, 0x4c, 0x95, 0x02, 0x77, 0xcf, 0xff, 0xff,
    0xc3, 0x44, 0x20, 0x44, 0x00, 0x8e, 0xb8, 0xa1, 0x09, 0x26, 0x03, 0xe0, 0xb5, 0x02, 0x95, 0x02,
    0x95, 0x2e, 0x7e, 0x05, 0xb5, 0xc2, 0x70, 0xcf, 0xff, 0xff, 0xc6, 0x10, 0x09, 0x9a, 0x04, 0xa0,
    0xb0, 0x26, 0x0e, 0x02, 0x05, 0x60, 0xde, 0x00, 0x0a, 0x12, 0x03, 0x20, 0xb7, 0xc4, 0x0b, 0x36,
    0x03, 0xa0, 0x70, 0xc9, 0x95, 0x02, 0x76, 0x08, 0xb8, 0xa8, 0xb5, 0x02, 0x70, 0xcf, 0x00, 0x00,
    0x55, 0x36, 0x78, 0x60, 0x26, 0x86, 0x1f, 0xfb, 0x95, 0x02, 0x78, 0xc5, 0x06, 0x31, 0x05, 0xe0,
    0xb5, 0x02, 0x72, 0xcf, 0xff, 0xff, 0xc5, 0xd4, 0x92, 0x3a, 0x73, 0xcf, 0xff, 0xff, 0xc7, 0xd0,
    0xb0, 0x20, 0x92, 0x20, 0xb0, 0x21, 0x92, 0x21, 0xb0, 0x22, 0x92, 0x22, 0xb0, 0x23, 0x92, 0x23,
    0xb0, 0x24, 0x92, 0x27, 0xb0, 0x25, 0x92, 0x28, 0xb0, 0x26, 0x92, 0x2b, 0xb0, 0x27, 0x92, 0x2c,
    0xb0, 0x28, 0x12, 0x58, 0x01, 0x01, 0xb0, 0x29, 0x12, 0x5a, 0x01, 0x01, 0xb0, 0x2a, 0x13, 0x36,
    0x80, 0x81, 0xa8, 0x36, 0x13, 0x37, 0x80, 0x81, 0xa8, 0x37, 0x12, 0xa7, 0x07, 0x01, 0xb0, 0x2c,
    0x13, 0x54, 0x80, 0x81, 0x7f, 0xe0, 0xa8, 0x3a, 0x78, 0xe0, 0xc0, 0xf1, 0x0d, 0xc2, 0x05, 0xc0,
    0x76, 0x08, 0x09, 0xbb, 0x00, 0x10, 0x75, 0xcf, 0xff, 0xff, 0xd4, 0xe0, 0x8d, 0x21, 0x8d, 0x00,
    0x21, 0x53, 0x00, 0x03, 0xb8, 0xc0, 0x8d, 0x45, 0x0b, 0x23, 0x00, 0x00, 0xea, 0x8f, 0x09, 0x15,
    0x00, 0x1e, 0xff, 0x81, 0xe8, 0x08, 0x25, 0x40, 0x19, 0x00, 0xff, 0xde, 0x8d, 0x00, 0xb8, 0x80,
    0xf0, 0x04, 0x8d, 0x00, 0xb8, 0xa0, 0xad, 0x00, 0x8d, 0x05, 0xe0, 0x81, 0x20, 0xcc, 0x80, 0xa2,
    0xdf, 0x00, 0xf4, 0x0a, 0x71, 0xcf, 0xff, 0xff, 0xc8, 0x4c, 0x91, 0x02, 0x77, 0x08, 0xb8, 0xa6,
    0x27, 0x86, 0x1f, 0xfe, 0xb1, 0x02, 0x0b, 0x42, 0x01, 0x80, 0x0e, 0x3e, 0x01, 0x80, 0x0f, 0x4a,
    0x01, 0x60, 0x70, 0xc9, 0x8d, 0x05, 0xe0, 0x81, 0x20, 0xcc, 0x80, 0xa2, 0xf4, 0x29, 0x76, 0xcf,
    0xff, 0xff, 0xc8, 0x4c, 0x08, 0x2d, 0x00, 0x51, 0x70, 0xcf, 0xff, 0xff, 0xc9, 0x0c, 0x88, 0x05,
    0x09, 0xb6, 0x03, 0x60, 0xd9, 0x08, 0x20, 0x99, 0x08, 0x02, 0x96, 0x34, 0xb5, 0x03, 0x79, 0x02,
    0x15, 0x23, 0x10, 0x80, 0xb6, 0x34, 0xe0, 0x01, 0x1d, 0x23, 0x10, 0x02, 0xf0, 0x0b, 0x96, 0x34,
    0x95, 0x03, 0x60, 0x38, 0xb6, 0x14, 0x15, 0x3f, 0x10, 0x80, 0xe0, 0x01, 0x1d, 0x3f, 0x10, 0x02,
    0xff, 0xa4, 0x96, 0x02, 0x7f, 0x05, 0xd8, 0x00, 0xb6, 0xe2, 0xad, 0x05, 0x05, 0x11, 0x05, 0xe0,
    0xd8, 0x00, 0xc0, 0xf1, 0x0c, 0xfe, 0x05, 0xc0, 0x0a, 0x96, 0x05, 0xa0, 0x76, 0x08, 0x0c, 0x22,
    0x02, 0x40, 0xe0, 0x80, 0x20, 0xca, 0x0f, 0x82, 0x00, 0x00, 0x19, 0x0b, 0x0c, 0x60, 0x05, 0xa2,
    0x21, 0xca, 0x00, 0x22, 0x0c, 0x56, 0x02, 0x40, 0xe8, 0x06, 0x0e, 0x0e, 0x02, 0x20, 0x70, 0xc9,
    0xf0, 0x48, 0x08, 0x96, 0x04, 0x40, 0x0e, 0x96, 0x04, 0x00, 0x09, 0x66, 0x03, 0x80, 0x75, 0xcf,
    0xff, 0xff, 0xd4, 0xe0, 0x8d, 0x00, 0x08, 0x4d, 0x00, 0x1e, 0xff, 0x47, 0x08, 0x0d, 0x00, 0x50,
    0xff, 0x57, 0x08, 0x41, 0x00, 0x51, 0x8d, 0x04, 0x95, 0x21, 0xe0, 0x64, 0x79, 0x0c, 0x70, 0x2f,
    0x0c, 0xe2, 0x05, 0xe0, 0xd9, 0x64, 0x72, 0xcf, 0xff, 0xff, 0xc7, 0x00, 0x92, 0x35, 0x08, 0x11,
    0x00, 0x43, 0xff, 0x3d, 0x08, 0x0d, 0x00, 0x51, 0xd8, 0x01, 0xff, 0x77, 0xf0, 0x25, 0x95, 0x01,
    0x92, 0x35, 0x09, 0x11, 0x00, 0x03, 0xff, 0x49, 0x08, 0x0d, 0x00, 0x51, 0xd8, 0x00, 0xff, 0x72,
    0xf0, 0x1b, 0x08, 0x86, 0x03, 0xe0, 0xd8, 0x01, 0x0e, 0xf6, 0x03, 0xc0, 0x0f, 0x52, 0x03, 0x40,
    0x0d, 0xba, 0x02, 0x00, 0x0a, 0xf6, 0x04, 0x40, 0x0c, 0x22, 0x04, 0x00, 0x0d, 0x72, 0x04, 0x40,
    0x0d, 0xc2, 0x02, 0x00, 0x09, 0x72, 0x04, 0x40, 0x0d, 0x3a, 0x02, 0x20, 0xd8, 0x20, 0x0b, 0xfa,
    0x02, 0x60, 0x70, 0xc9, 0x04, 0x51, 0x05, 0xc0, 0x78, 0xe0, 0xd9, 0x00, 0xf0, 0x0a, 0x70, 0xcf,
    0xff, 0xff, 0xd5, 0x20, 0x78, 0x35, 0x80, 0x41, 0x80, 0x00, 0xe1, 0x02, 0xa0, 0x40, 0x09, 0xf1,
    0x81, 0x14, 0x71, 0xcf, 0xff, 0xff, 0xd4, 0xe0, 0x70, 0xcf, 0xff, 0xff, 0xc5, 0x94, 0xb0, 0x3a,
    0x7f, 0xe0, 0xd8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x00, 0x05, 0x00, 0x02, 0x00, 0x03, 0x30,
    0x00, 0x00, 0x00, 0x00, 0x03, 0xcd, 0x05, 0x0d, 0x01, 0xc5, 0x03, 0xb3, 0x00, 0xe0, 0x01, 0xe3,
    0x02, 0x80, 0x01, 0xe0, 0x01, 0x09, 0x00, 0x80, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xc9, 0xb4, 0xff, 0xff, 0xd3, 0x24, 0xff, 0xff,
    0xca, 0x34, 0xff, 0xff, 0xd3, 0xec
};
static const uint8_t patch_0402[] = {
    0xd5, 0x30, 0xc0, 0xf1, 0x0b, 0xba, 0x05, 0xc0, 0xc1, 0xa2, 0x75, 0xcf, 0xff, 0xff, 0xc7, 0xc0,
    0xde, 0x00, 0x1c, 0x05, 0x33, 0x82, 0xc6, 0x61, 0x08, 0xc3, 0x00, 0x40, 0xe1, 0xd1, 0xf2, 0x17,
    0xe1, 0xd4, 0xf4, 0x5b, 0x8d, 0x01, 0x08, 0xb3, 0x00, 0x1e, 0x1c, 0x00, 0x3f, 0x80, 0x00, 0x0c,
    0x35, 0x00, 0x71, 0x8b, 0x0f, 0xbe, 0x04, 0xe0, 0x70, 0xc9, 0x08, 0x06, 0x05, 0x20, 0x70, 0xc9,
    0x95, 0x25, 0x70, 0xcf, 0xff, 0x00, 0x00, 0x00, 0xb0, 0x39, 0xf0, 0x47, 0x8d, 0x01, 0x08, 0x27,
    0x00, 0xdf, 0x8d, 0x02, 0x08, 0x1d, 0x00, 0x1e, 0x8d, 0x02, 0xad, 0x01, 0x8d, 0x01, 0xb8, 0xa3,
    0xad, 0x01, 0x70, 0xcf, 0xff, 0x00, 0x00, 0x00, 0x90, 0x19, 0xb5, 0x05, 0xf0, 0x05, 0x8d, 0x01,
    0xb8, 0xa0, 0xad, 0x01, 0x8d, 0x01, 0x08, 0x11, 0x00, 0xde, 0x8d, 0x01, 0xb8, 0xa3, 0xad, 0x01,
    0x0d, 0x02, 0x04, 0xa0, 0xd8, 0x01, 0x8d, 0x01, 0x08, 0x53, 0x00, 0x1e, 0x85, 0x01, 0x0b, 0x5e,
    0x05, 0xe0, 0x21, 0x8a, 0x04, 0x1f, 0x95, 0x24, 0x29, 0x05, 0x00, 0x3e, 0x1c, 0x00, 0x3e, 0x40,
    0x71, 0x8b, 0x0f, 0x4e, 0x04, 0xe0, 0xd8, 0x00, 0x0f, 0x9a, 0x04, 0xe0, 0xd8, 0x00, 0xd8, 0x00,
    0x0f, 0xb6, 0x04, 0xe0, 0xd9, 0x01, 0x8d, 0x01, 0x08, 0x23, 0x00, 0x5f, 0x20, 0x8a, 0x00, 0x1c,
    0x0b, 0xe2, 0x04, 0xe0, 0xd9, 0x00, 0xd8, 0x0d, 0xb8, 0x0a, 0x0b, 0xd6, 0x04, 0xe0, 0xd9, 0x00,
    0xd8, 0x90, 0x0b, 0xea, 0x04, 0xe0, 0xd9, 0x01, 0xd8, 0x00, 0x02, 0xf5, 0x05, 0xe0, 0xc0, 0xa2,
    0x78, 0xe0, 0xc0, 0xf1, 0x73, 0xcf, 0xff, 0xff, 0xc7, 0xc0, 0x8b, 0x41, 0x0a, 0x0d, 0x00, 0xde,
    0x8b, 0x41, 0x0a, 0x0f, 0x00, 0x5e, 0x72, 0xcf, 0x00, 0x00, 0x40, 0x3e, 0x7a, 0x40, 0xf0, 0x02,
    0xd8, 0x00, 0xc0, 0xd1, 0x7e, 0xe0, 0xc0, 0xf1, 0xc5, 0xe1, 0x75, 0xcf, 0xff, 0xff, 0xc7, 0xc0,
    0xd9, 0x00, 0xf0, 0x09, 0x70, 0xcf, 0xff, 0xff, 0xd7, 0x40, 0x78, 0x35, 0x80, 0x41, 0x80, 0x00,
    0xe1, 0x02, 0xa0, 0x40, 0x09, 0xf3, 0x80, 0x94, 0x71, 0xcf, 0xff, 0xff, 0xd6, 0xd0, 0xd8, 0x03,
    0x0d, 0x1e, 0x04, 0xa0, 0xda, 0x00, 0x70, 0xcf, 0xff, 0xff, 0xd5, 0x30, 0x0c, 0x22, 0x03, 0x00,
    0xe8, 0x87, 0x70, 0xcf, 0xff, 0xff, 0xd6, 0xe8, 0x09, 0x4a, 0x02, 0x40, 0x21, 0x8a, 0x0a, 0x0f,
    0xb5, 0x24, 0xd9, 0x04, 0xad, 0x23, 0x1d, 0x04, 0x1f, 0x80, 0x01, 0x6e, 0x36, 0x00, 0x02, 0x79,
    0x05, 0xc0, 0xc0, 0xf1, 0x71, 0xcf, 0xff, 0xff, 0xc3, 0x50, 0x11, 0x09, 0x00, 0xc0, 0x73, 0xcf,
    0xff, 0xff, 0xc1, 0x64, 0xe0, 0xd2, 0x72, 0xcf, 0xff, 0xff, 0xc7, 0xc0, 0xf4, 0x12, 0x8b, 0x08,
    0x08, 0x21, 0x03, 0xd1, 0x8a, 0x01, 0x08, 0x19, 0x00, 0xdf, 0xd8, 0x54, 0xa9, 0x0b, 0xd8, 0x00,
    0xa9, 0x08, 0x8a, 0x21, 0xb9, 0x83, 0xaa, 0x21, 0xaa, 0x0c, 0x0b, 0xe6, 0x04, 0x80, 0xf1, 0xb2,
    0x78, 0xe0, 0xc0, 0xf1, 0x70, 0xcf, 0xff, 0xff, 0xc7, 0xc0, 0x88, 0x01, 0xb8, 0xe0, 0x0f, 0xb4,
    0xff, 0xc2, 0x0e, 0x9a, 0x02, 0x40, 0xf1, 0xa6, 0x78, 0xe0, 0xc0, 0xf1, 0x71, 0xcf, 0xff, 0xff,
    0xc7, 0xc0, 0x89, 0x61, 0x72, 0xcf, 0xff, 0xff, 0xc3, 0x50, 0x0b, 0x45, 0x00, 0xde, 0x08, 0x41,
    0x08, 0x11, 0x89, 0x0c, 0xe0, 0x01, 0xa9, 0x0c, 0x89, 0x6c, 0x89, 0x03, 0x0b, 0x0d, 0x00, 0x03,
    0x89, 0x02, 0x08, 0x2d, 0x00, 0x9e, 0x89, 0x02, 0x08, 0x13, 0x00, 0x9e, 0x89, 0x02, 0xb8, 0xa2,
    0xa9, 0x02, 0x89, 0x01, 0xb8, 0x82, 0xf0, 0x03, 0x89, 0x01, 0xb8, 0xa2, 0xa9, 0x01, 0xd8, 0x50,
    0xaa, 0x0b, 0xd9, 0x00, 0xaa, 0x2a, 0xaa, 0x28, 0x0c, 0xca, 0x02, 0xe0, 0x82, 0x00, 0xd8, 0x00,
    0xf1, 0x7a, 0xff, 0xff, 0xca, 0xd0, 0xff, 0xff, 0xd6, 0x10
};
static const uint8_t patch_0502[] = {
    0xd7, 0x48, 0x72, 0xcf, 0xff, 0xff, 0xc7, 0xc0, 0x08, 0x27, 0x00, 0x40, 0xe1, 0xd1, 0xf4, 0x0f,
    0x8a, 0x01, 0x08, 0x1b, 0x00, 0xdf, 0x70, 0xcf, 0xff, 0xff, 0xc6, 0x44, 0x88, 0x08, 0xb8, 0xe4,
    0x8a, 0x0e, 0x20, 0xcf, 0x00, 0x62, 0x20, 0xd0, 0x00, 0x61, 0xaa, 0x0e, 0x7f, 0xe0, 0xd8, 0x00,
    0x78, 0xe0, 0xc0, 0xf1, 0x09, 0x6e, 0x05, 0xc0, 0x75, 0xcf, 0xff, 0xff, 0xc7, 0xc0, 0x70, 0xcf,
    0x00, 0x00, 0x55, 0x36, 0x78, 0x60, 0xc1, 0xa1, 0x95, 0xca, 0x77, 0xcf, 0xff, 0xff, 0xc8, 0xd4,
    0x9f, 0x1c, 0x08, 0x27, 0x00, 0x12, 0xd9, 0x08, 0x0d, 0x56, 0x03, 0x20, 0xda, 0x05, 0x9f, 0x3c,
    0x09, 0x13, 0x0f, 0x82, 0x00, 0x00, 0x0b, 0x00, 0xb8, 0x2a, 0x7e, 0x0c, 0x76, 0x2f, 0xf0, 0x0f,
    0x7e, 0x0c, 0x29, 0x41, 0x72, 0x8e, 0xf0, 0x0b, 0x78, 0x13, 0xd9, 0x08, 0x0d, 0x32, 0x03, 0x20,
    0xda, 0x05, 0x71, 0x08, 0x09, 0x52, 0x05, 0xe0, 0x70, 0xc9, 0x76, 0x08, 0x8d, 0x0f, 0x08, 0x25,
    0x00, 0x1e, 0x70, 0xcf, 0xff, 0xff, 0xc8, 0x2f, 0x0d, 0x1e, 0x04, 0x00, 0x28, 0x05, 0x03, 0xbe,
    0x70, 0xcf, 0xff, 0xff, 0xc9, 0x14, 0x90, 0x33, 0xb9, 0x24, 0x09, 0x2a, 0x05, 0xe0, 0x70, 0x2f,
    0x76, 0x08, 0x8d, 0x0e, 0x08, 0x83, 0x00, 0x5e, 0x8d, 0x01, 0x08, 0x7b, 0x00, 0xde, 0x8d, 0x0e,
    0x77, 0xcf, 0xff, 0xff, 0xc8, 0x4c, 0xb8, 0xa0, 0xad, 0x0e, 0x8f, 0x27, 0x8f, 0x49, 0x22, 0x02,
    0x80, 0x40, 0x00, 0x08, 0x00, 0x03, 0x21, 0x02, 0x00, 0x80, 0x78, 0x0e, 0xc0, 0x40, 0x8f, 0x08,
    0x78, 0x2c, 0x70, 0x2f, 0xe0, 0x63, 0x08, 0xfe, 0x05, 0xe0, 0xd9, 0x64, 0x97, 0x20, 0x78, 0x0d,
    0xb9, 0xc1, 0x21, 0x42, 0x80, 0x02, 0x22, 0xca, 0x00, 0x62, 0xc3, 0x00, 0x72, 0x59, 0x08, 0x17,
    0x00, 0xe3, 0xd9, 0x00, 0x70, 0xcf, 0xff, 0xff, 0xd8, 0xba, 0x88, 0x00, 0xe0, 0x80, 0x22, 0xcc,
    0x90, 0x22, 0xf2, 0x02, 0xd9, 0x01, 0x26, 0x2f, 0xf0, 0x47, 0xf2, 0x08, 0x8d, 0x0e, 0xb8, 0x80,
    0xad, 0x0e, 0xa5, 0xc4, 0x8d, 0x02, 0xb8, 0x82, 0xad, 0x02, 0x70, 0xcf, 0xff, 0xff, 0xd8, 0xba,
    0xa8, 0x40, 0xf0, 0x02, 0xa5, 0xc4, 0x00, 0x79, 0x05, 0xe0, 0xc0, 0xa1, 0x78, 0xe0, 0xc0, 0xf1,
    0xc5, 0xe1, 0x75, 0xcf, 0xff, 0xff, 0xc7, 0xc0, 0xd9, 0x00, 0xf0, 0x09, 0x70, 0xcf, 0xff, 0xff,
    0xd8, 0xbc, 0x78, 0x35, 0x80, 0x41, 0x80, 0x00, 0xe1, 0x02, 0xa0, 0x40, 0x09, 0xf3, 0x80, 0x94,
    0x70, 0xcf, 0xff, 0xff, 0xd7, 0x48, 0x09, 0xde, 0x03, 0x00, 0x21, 0x8a, 0x00, 0x14, 0xb5, 0x2a,
    0x00, 0x51, 0x05, 0xc0, 0x00, 0x00, 0xff, 0xff, 0xcb, 0x54, 0xff, 0xff, 0xd7, 0x78
};
static const uint8_t awb_ccm[] = {
    0xc8, 0x92, 0x02, 0x67, 0xff, 0x1a, 0xff, 0xb3, 0xff, 0x80, 0x01, 0x66, 0x00, 0x03, 0xff, 0x9a,
    0xfe, 0xb4, 0x02, 0x4d, 0x01, 0xbf, 0xff, 0x01, 0xff, 0xf3, 0xff, 0x75, 0x01, 0x98, 0xff, 0xfd,
    0xff, 0x9a, 0xfe, 0xe7, 0x02, 0xa8, 0x01, 0xd9, 0xff, 0x26, 0xff, 0xf3, 0xff, 0xb3, 0x01, 0x32,
    0xff, 0xe8, 0xff, 0xda, 0xfe, 0xcd, 0x02, 0xc2
};
static const uint8_t awb_weights[] = {
    0xc8, 0xf4, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xe7, 0x24, 0x15, 0x83, 0x20, 0x45, 0x03, 0xff,
    0x00, 0x7c
};
static const uint16_t cpipe_regs_8_bit_a[] = {
    0xC92A, // CAM_LL_START_SATURATION
    0xC92B, // CAM_LL_END_SATURATION
    0xC92C, // CAM_LL_START_DESATURATION
    0xC92D, // CAM_LL_END_DESATURATION
    0xC92E, // CAM_LL_START_DEMOSAIC
    0xC92F, // CAM_LL_START_AP_GAIN
    0xC930, // CAM_LL_START_AP_THRESH
    0xC931, // CAM_LL_STOP_DEMOSAIC
    0xC932, // CAM_LL_STOP_AP_GAIN
    0xC933, // CAM_LL_STOP_AP_THRESH
    0xC934, // CAM_LL_START_NR_RED
    0xC935, // CAM_LL_START_NR_GREEN
    0xC936, // CAM_LL_START_NR_BLUE
    0xC937, // CAM_LL_START_NR_THRESH
    0xC938, // CAM_LL_STOP_NR_RED
    0xC939, // CAM_LL_STOP_NR_GREEN
    0xC93A, // CAM_LL_STOP_NR_BLUE
    0xC93B, // CAM_LL_STOP_NR_THRESH
    0xC942, // CAM_LL_START_CONTRAST_GRADIENT
    0xC943, // CAM_LL_STOP_CONTRAST_GRADIENT
    0xC944, // CAM_LL_START_CONTRAST_LUMA_PERCENTAGE
    0xC945, // CAM_LL_STOP_CONTRAST_LUMA_PERCENTAGE
    0xC950, // CAM_LL_CLUSTER_DC_GATE_PERCENTAGE
    0xC951, // CAM_LL_SUMMING_SENSITIVITY_FACTOR
    0xC87B, // CAM_AET_TARGET_AVERAGE_LUMA_DARK
    0xC878, // CAM_AET_AEMODE
    0xB42A, // CCM_DELTA_GAIN
    0xA80A, // AE_TRACK_AE_TRACKING_DAMPENING_SPEED
};

static const uint16_t cpipe_regs_8_bit_d[] = {
    0x80, // CAM_LL_START_SATURATION
    0x4B, // CAM_LL_END_SATURATION
    0x00, // CAM_LL_START_DESATURATION
    0xFF, // CAM_LL_END_DESATURATION
    0x3C, // CAM_LL_START_DEMOSAIC
    0x02, // CAM_LL_START_AP_GAIN
    0x06, // CAM_LL_START_AP_THRESH
    0x64, // CAM_LL_STOP_DEMOSAIC
    0x01, // CAM_LL_STOP_AP_GAIN
    0x0C, // CAM_LL_STOP_AP_THRESH
    0x3C, // CAM_LL_START_NR_RED
    0x3C, // CAM_LL_START_NR_GREEN
    0x3C, // CAM_LL_START_NR_BLUE
    0x0F, // CAM_LL_START_NR_THRESH
    0x64, // CAM_LL_STOP_NR_RED
    0x64, // CAM_LL_STOP_NR_GREEN
    0x64, // CAM_LL_STOP_NR_BLUE
    0x32, // CAM_LL_STOP_NR_THRESH
    0x38, // CAM_LL_START_CONTRAST_GRADIENT
    0x30, // CAM_LL_STOP_CONTRAST_GRADIENT
    0x50, // CAM_LL_START_CONTRAST_LUMA_PERCENTAGE
    0x19, // CAM_LL_STOP_CONTRAST_LUMA_PERCENTAGE
    0x05, // CAM_LL_CLUSTER_DC_GATE_PERCENTAGE
    0x40, // CAM_LL_SUMMING_SENSITIVITY_FACTOR
    0x1B, // CAM_AET_TARGET_AVERAGE_LUMA_DARK
    0x00, // CAM_AET_AEMODE (was 0x0E)
    0x05, // CCM_DELTA_GAIN
    0x20  // AE_TRACK_AE_TRACKING_DAMPENING_SPEED
};

static const uint16_t cpipe_regs_16_bit[][2] = {
    {0xC926, 0x0020}, // CAM_LL_START_BRIGHTNESS
    {0xC928, 0x009A}, // CAM_LL_STOP_BRIGHTNESS
    {0xC946, 0x0070}, // CAM_LL_START_GAIN_METRIC
    {0xC948, 0x00F3}, // CAM_LL_STOP_GAIN_METRIC
    {0xC952, 0x0020}, // CAM_LL_START_TARGET_LUMA_BM
    {0xC954, 0x009A}, // CAM_LL_STOP_TARGET_LUMA_BM
    {0xC93C, 0x0020}, // CAM_LL_START_CONTRAST_BM
    {0xC93E, 0x009A}, // CAM_LL_STOP_CONTRAST_BM
    {0xC940, 0x00DC}, // CAM_LL_GAMMA
    {0xC94A, 0x0230}, // CAM_LL_START_FADE_TO_BLACK_LUMA
    {0xC94C, 0x0010}, // CAM_LL_STOP_FADE_TO_BLACK_LUMA
    {0xC94E, 0x01CD}, // CAM_LL_CLUSTER_DC_TH_BM
    {0xC890, 0x0080}, // CAM_AET_TARGET_GAIN
    {0xC886, 0x0100}, // CAM_AET_AE_MAX_VIRT_AGAIN
    {0xC87C, 0x005A}  // CAM_AET_BLACK_CLIPPING_TARGET
};

static int host_command(sensor_t *sensor, uint16_t command) {
    if (omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_HOST_COMMAND, (command | MT9M114_HC_OK)) != 0) {
        return -1;
    }

    for (uint32_t start = rt_tick_get();; rt_thread_mdelay(MT9M114_HC_DELAY)) {
        uint16_t reg_data;

        if (omv_i2c_readw2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_HOST_COMMAND, &reg_data) != 0) {
            return -1;
        }

        if ((reg_data & command) == 0) {
            return (reg_data & MT9M114_HC_OK) ? 0 : -1;
        }

        if ((rt_tick_get() - start) >= MT9M114_HC_TIMEOUT) {
            return -1;
        }
    }

    return 0;
}

static int load_patch(sensor_t *sensor, const uint8_t *patch, size_t patch_len,
                      uint16_t patch_loader_address, uint16_t patch_id, uint32_t patch_firmware_id) {
    int ret = 0;
    // Patch address is stashed in the first two bytes.
    uint16_t patch_address = (((uint16_t) patch[0]) << 8) | patch[1];
    ret |= omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_XMDA_ACCESS_CTL_STAT, patch_address >> 15);
    ret |=
        omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_XMDA_PHYSICAL_ADDRESS_ACCESS, patch_address & 0x7FFF);
    ret |= omv_i2c_write_bytes(&sensor->i2c_bus, sensor->slv_addr, (uint8_t *) patch, patch_len, OMV_I2C_XFER_NO_FLAGS);
    ret |= omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_XMDA_LOGIC_ADDRESS_ACCESS, 0x0000);
    ret |= omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_PATCHLDR_LOADER_ADDRESS, patch_loader_address);
    ret |= omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_PATCHLDR_PATCH_ID, patch_id);
    ret |= omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_PATCHLDR_FIRMWARE_ID_HI, patch_firmware_id >> 16);
    ret |= omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_PATCHLDR_FIRMWARE_ID_LO, patch_firmware_id);

    ret |= host_command(sensor, MT9M114_HC_APPLY_PATCH);

    uint8_t reg_data;
    ret |= omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_PATCHLDR_APPLY_STATUS, &reg_data);
    ret |= (reg_data == 0) ? 0: -1;

    return ret;
}

static int load_awb_cmm(sensor_t *sensor) {
    return omv_i2c_write_bytes(&sensor->i2c_bus, sensor->slv_addr, (uint8_t *) awb_ccm, sizeof(awb_ccm), OMV_I2C_XFER_NO_FLAGS);
}

static int load_awb(sensor_t *sensor) {
    int ret = 0;

    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_CAM_AWB_XSCALE, 0x03);
    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_CAM_AWB_YSCALE, 0x02);
    ret |= omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_CAM_AWB_Y_SHIFT_PRE_ADJ, 0x003C);
    ret |= omv_i2c_write_bytes(&sensor->i2c_bus, sensor->slv_addr,
                               (uint8_t *) awb_weights, sizeof(awb_weights), OMV_I2C_XFER_NO_FLAGS);

    for (int i = 0xC90C; i <= 0xC911; i++) {
        ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, i, 0x80);
    }

    return ret;
}

static int load_cpipe(sensor_t *sensor) {
    int ret = 0;

    int a_size = sizeof(cpipe_regs_8_bit_a) / sizeof(cpipe_regs_8_bit_a[0]);
    int d_size = sizeof(cpipe_regs_8_bit_d) / sizeof(cpipe_regs_8_bit_d[0]);

    for (int i = 0, ii = IM_MIN(a_size, d_size); i < ii; i++) {
        ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, cpipe_regs_8_bit_a[i], cpipe_regs_8_bit_d[i]);
    }

    for (int i = 0; i < (sizeof(cpipe_regs_16_bit) / sizeof(cpipe_regs_16_bit[0])); i++) {
        ret |= omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, cpipe_regs_16_bit[i][0], cpipe_regs_16_bit[i][1]);
    }

    return ret;
}

static int set_system_state(sensor_t *sensor, uint8_t state) {
    if (omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_SYSMGR_NEXT_STATE, state) != 0) {
        return -1;
    }

    return (host_command(sensor, MT9M114_HC_SET_STATE) == 0) ? 0 : -1;
}

static int change_config(sensor_t *sensor) {
    return set_system_state(sensor, MT9M114_SS_ENTER_CONFIG_CHANGE);
}

static int refresh(sensor_t *sensor) {
    int ret = host_command(sensor, MT9M114_HC_REFRESH);

    if (ret == 0) {
        return ret;
    }

    uint8_t reg_data;

    if (omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_SEQ_ERROR_CODE, &reg_data) != 0) {
        return -1;
    }

    return -reg_data;
}

static int set_pixformat(sensor_t *sensor, pixformat_t pixformat);
static int set_framesize(sensor_t *sensor, framesize_t framesize);
static int reset(sensor_t *sensor) {
    int ret = 0;
    readout_x = 0;
    readout_y = 0;

    readout_w = ACTIVE_SENSOR_WIDTH;
    readout_h = ACTIVE_SENSOR_HEIGHT;

    ret |= omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_SYSCTL, MT9M114_SYSCTL_SOFT_RESET);
    rt_thread_mdelay(1);

    ret |= omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_SYSCTL, 0);
    rt_thread_mdelay(45);

    for (uint32_t start = rt_tick_get();; rt_thread_mdelay(MT9M114_HC_DELAY)) {
        uint16_t reg_data;

        if (omv_i2c_readw2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_HOST_COMMAND, &reg_data) != 0) {
            return -1;
        }

        if ((reg_data & MT9M114_HC_SET_STATE) != 0) {
            break;
        }

        if ((rt_tick_get() - start) >= MT9M114_HC_TIMEOUT) {
            return -1;
        }
    }

    for (uint32_t start = rt_tick_get();; rt_thread_mdelay(MT9M114_HC_DELAY)) {
        uint8_t reg_data;

        if (omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_SYSMGR_CURRENT_STATE, &reg_data) != 0) {
            return -1;
        }

        if (reg_data != MT9M114_SS_STANDBY) {
            break;
        }

        if ((rt_tick_get() - start) >= MT9M114_HC_TIMEOUT) {
            return -1;
        }
    }

    // Errata 2 (Black Frame Output)
    uint16_t reg;
    ret |= omv_i2c_readw2(&sensor->i2c_bus, sensor->slv_addr, 0x301A, &reg);
    ret |= omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, 0x301A, reg | (1 << 9));

    ret |= omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_CAM_SYSCTL_PLL_DIVIDER_M_N,
                           (sensor_get_xclk_frequency() == MT9M114_XCLK_FREQ)
            ? 0x120 // xclk=24MHz, m=32, n=1, sensor=48MHz, bus=76.8MHz
            : 0x448); // xclk=25MHz, m=72, n=4, sensor=45MHz, bus=72MHz

    for (int i = 0; i < (sizeof(default_regs) / sizeof(default_regs[0])); i++) {
        ret |= omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, default_regs[i][0], default_regs[i][1]);
    }

    ret |= set_framesize(sensor, FRAMESIZE_SXGAM);

    // Black level correction fix
    ret |= load_patch(sensor, patch_0202, sizeof(patch_0202), 0x010c, 0x0202, 0x41030202);
    // Adaptive Sensitivity
    ret |= load_patch(sensor, patch_0302, sizeof(patch_0302), 0x04b4, 0x0302, 0x41030202);
    // System Idle Control (auto wakeup from standby)
    ret |= load_patch(sensor, patch_0402, sizeof(patch_0402), 0x0634, 0x0402, 0x41030202);
    // Ambient light sensor
    ret |= load_patch(sensor, patch_0502, sizeof(patch_0502), 0x0884, 0x0502, 0x41030202);

    ret |= load_awb_cmm(sensor);
    ret |= load_awb(sensor);
    ret |= load_cpipe(sensor);

    ret |= omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_CAM_PORT_OUTPUT_CONTROL, 0x8008);
    ret |= omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_PAD_SLEW, 0x0777);
    ret |= omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_SENSOR_CONTROL_READ_MODE,
                           MT9M114_SENSOR_CONTROL_READ_MODE_HMIRROR | MT9M114_SENSOR_CONTROL_READ_MODE_VFLIP);

    ret |= change_config(sensor);

    return ret;
}

static int sleep(sensor_t *sensor, int enable) {
    uint8_t state = MT9M114_SS_LEAVE_STANDBY;
    uint8_t new_state = MT9M114_SS_STREAMING;

    if (enable) {
        state = MT9M114_SS_ENTER_STANDBY;
        new_state = MT9M114_SS_STANDBY;
    }

    if (set_system_state(sensor, state) != 0) {
        return -1;
    }

    for (uint32_t start = rt_tick_get();; rt_thread_mdelay(MT9M114_HC_DELAY)) {
        uint8_t reg_data;

        if (omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_SYSMGR_CURRENT_STATE, &reg_data) != 0) {
            return -1;
        }

        if (reg_data == new_state) {
            return 0;
        }

        if ((rt_tick_get() - start) >= MT9M114_HC_TIMEOUT) {
            return -1;
        }
    }
}


static int read_reg(sensor_t *sensor, uint16_t reg_addr) {
    uint16_t reg_data;

    if (omv_i2c_readw2(&sensor->i2c_bus, sensor->slv_addr, reg_addr, &reg_data) != 0) {
        return -1;
    }

    return reg_data;
}

static int write_reg(sensor_t *sensor, uint16_t reg_addr, uint16_t reg_data) {
    return omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, reg_addr, reg_data);
}

static int set_pixformat(sensor_t *sensor, pixformat_t pixformat) {
    uint16_t reg = 0;

    switch (pixformat) {
        case PIXFORMAT_GRAYSCALE:
        case PIXFORMAT_YUV422:
            reg = MT9M114_OUTPUT_FORMAT_YUV | MT9M114_OUTPUT_FORMAT_SWAP_BYTES;
            break;
        case PIXFORMAT_RGB565:
            reg = MT9M114_OUTPUT_FORMAT_RGB | MT9M114_OUTPUT_FORMAT_RGB565 | MT9M114_OUTPUT_FORMAT_SWAP_BYTES;
            break;
        case PIXFORMAT_BAYER:
            if ((sensor->framesize != FRAMESIZE_INVALID)
                && (sensor->framesize != FRAMESIZE_VGA)
                && (sensor->framesize != FRAMESIZE_SXGAM)) {
                return -1;
            }
            reg = MT9M114_OUTPUT_FORMAT_BAYER | MT9M114_OUTPUT_FORMAT_RAW_BAYER_10;
            break;
        default:
            return -1;
    }

    if (omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_CAM_OUTPUT_FORMAT, reg) != 0) {
        return -1;
    }

    return change_config(sensor);
}

static int set_framesize(sensor_t *sensor, framesize_t framesize) {
    int ret = 0;
    uint16_t w = resolution[framesize][0];
    uint16_t h = resolution[framesize][1];

    if ((sensor->pixformat == PIXFORMAT_BAYER) && ((framesize != FRAMESIZE_VGA) && (framesize != FRAMESIZE_SXGAM))) {
        return -1;
    }

    if ((w > ACTIVE_SENSOR_WIDTH) || (h > ACTIVE_SENSOR_HEIGHT)) {
        return -1;
    }

    // Step 0: Clamp readout settings.

    readout_w = IM_MAX(readout_w, w);
    readout_h = IM_MAX(readout_h, h);

    int readout_x_max = (ACTIVE_SENSOR_WIDTH - readout_w) / 2;
    int readout_y_max = (ACTIVE_SENSOR_HEIGHT - readout_h) / 2;
    readout_x = IM_MAX(IM_MIN(readout_x, readout_x_max), -readout_x_max);
    readout_y = IM_MAX(IM_MIN(readout_y, readout_y_max), -readout_y_max);

    // Step 1: Determine readout area and subsampling amount.

    uint16_t read_mode;

    if (omv_i2c_readw2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_SENSOR_CONTROL_READ_MODE, &read_mode) != 0) {
        return -1;
    }

    int read_mode_div = 1;
    read_mode &= ~(MT9M114_SENSOR_CONTROL_READ_MODE_HBIN_MASK | MT9M114_SENSOR_CONTROL_READ_MODE_VBIN_MASK);

    if ((w <= (readout_w / 2)) && (h <= (readout_h / 2))) {
        read_mode_div = 2;
        read_mode |= MT9M114_SENSOR_CONTROL_READ_MODE_HBIN | MT9M114_SENSOR_CONTROL_READ_MODE_VBIN;
    }

    // Step 2: Determine horizontal and vertical start and end points.

    uint16_t sensor_w = readout_w + DUMMY_WIDTH_BUFFER; // camera hardware needs dummy pixels to sync
    uint16_t sensor_h = readout_h + DUMMY_HEIGHT_BUFFER; // camera hardware needs dummy lines to sync

    uint16_t sensor_ws = IM_MAX(IM_MIN((((ACTIVE_SENSOR_WIDTH - sensor_w) / 4) + (readout_x / 2)) * 2,
                                       ACTIVE_SENSOR_WIDTH - sensor_w),
                                -(DUMMY_WIDTH_BUFFER / 2)) + DUMMY_COLUMNS; // must be multiple of 2
    uint16_t sensor_we = sensor_ws + sensor_w - 1;

    int sensor_ws_mod = sensor_ws % (read_mode_div * 4); // multiple 4/8
    if (sensor_ws_mod) {
        sensor_ws -= sensor_ws_mod;
        sensor_we += read_mode_div;
    }

    uint16_t sensor_hs = IM_MAX(IM_MIN((((ACTIVE_SENSOR_HEIGHT - sensor_h) / 4) - (readout_y / 2)) * 2,
                                       ACTIVE_SENSOR_HEIGHT - sensor_h),
                                -(DUMMY_HEIGHT_BUFFER / 2)) + DUMMY_LINES; // must be multiple of 2
    uint16_t sensor_he = sensor_hs + sensor_h - 1;

    int sensor_hs_mod = sensor_hs % (read_mode_div * 4); // multiple 4/8
    if (sensor_hs_mod) {
        sensor_hs -= sensor_hs_mod;
        sensor_he += read_mode_div;
    }

    // Step 3: Determine scaling window offset.

    float ratio = IM_MIN((readout_w / read_mode_div) / ((float) w), (readout_h / read_mode_div) / ((float) h));

    uint16_t w_mul = w * ratio;
    uint16_t h_mul = h * ratio;
    uint16_t x_off = ((readout_w / read_mode_div) - w_mul) / 2;
    uint16_t y_off = ((readout_h / read_mode_div) - h_mul) / 2;

    // Step 4: Write regs.

    uint16_t frame_length_lines = (readout_h / read_mode_div) + ((read_mode_div == 2) ? 40 : 39) + DUMMY_HEIGHT_BUFFER;
    uint16_t line_length_pck = (readout_w / read_mode_div) + ((read_mode_div == 2) ? 534 : 323) + DUMMY_WIDTH_BUFFER;

    // Errata 4 (Cam_port_clock_slowdown issues/limitations)
    if (!(line_length_pck % 5)) {
        line_length_pck += 1;
    }

    ret |= omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_SENSOR_CFG_Y_ADDR_START, sensor_hs);
    ret |= omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_SENSOR_CFG_X_ADDR_START, sensor_ws);
    ret |= omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_SENSOR_CFG_Y_ADDR_END, sensor_he);
    ret |= omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_SENSOR_CFG_X_ADDR_END, sensor_we);

    int pixclk = (sensor_get_xclk_frequency() == MT9M114_XCLK_FREQ) ? 48000000 : 45000000;

    ret |= omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_SENSOR_CFG_PIXCLK, pixclk >> 16);
    ret |= omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_SENSOR_CFG_PIXCLK + 2, pixclk);

    ret |= omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_SENSOR_CFG_FINE_INTEG_TIME_MIN,
                           (read_mode_div == 2) ? 451 : 219); // figured this out by checking register wizard against datasheet
    ret |= omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_SENSOR_CFG_FINE_INTEG_TIME_MAX,
                           (read_mode_div == 2) ? 947 : 1480); // figured this out by checking register wizard against datasheet
    ret |= omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_SENSOR_CFG_FRAME_LENGTH_LINES,
                           frame_length_lines); // figured this out by checking register wizard against datasheet
    ret |= omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_SENSOR_CFG_LINE_LENGTH_PCK,
                           line_length_pck); // figured this out by checking register wizard against datasheet
    ret |= omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_SENSOR_CFG_FINE_CORRECTION,
                           (read_mode_div == 2) ? 224 : 96); // figured this out by checking register wizard against datasheet
    ret |= omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_SENSOR_CFG_CPIPE_LAST_ROW,
                           (readout_h / read_mode_div) + 3); // figured this out by checking register wizard against datasheet

    ret |= omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_SENSOR_CONTROL_READ_MODE, read_mode);

    ret |= omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_CROP_WINDOW_X_OFFSET, x_off);
    ret |= omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_CROP_WINDOW_Y_OFFSET, y_off);
    ret |= omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_CROP_WINDOW_WIDTH, w_mul);
    ret |= omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_CROP_WINDOW_HEIGHT, h_mul);

    ret |= omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_CAM_OUTPUT_WIDTH, w);
    ret |= omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_CAM_OUTPUT_HEIGHT, h);

    float rate = (((float) pixclk) / (frame_length_lines * line_length_pck)) * 256;

    ret |= omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_CAM_AET_MAX_FRAME_RATE, fast_ceilf(rate));
    ret |= omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_CAM_AET_MIN_FRAME_RATE, fast_floorf(rate / 2.f));

    ret |= omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_AWB_CLIP_WINDOW_X_START, 0);
    ret |= omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_AWB_CLIP_WINDOW_Y_START, 0);
    ret |= omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_AWB_CLIP_WINDOW_X_END, w - 1);
    ret |= omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_AWB_CLIP_WINDOW_Y_END, h - 1);

    ret |= omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_AE_INITIAL_WINDOW_X_START, 0);
    ret |= omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_AE_INITIAL_WINDOW_Y_START, 0);
    ret |= omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_AE_INITIAL_WINDOW_X_END, (w / 5) - 1);
    ret |= omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_AE_INITIAL_WINDOW_Y_END, (h / 5) - 1);

    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_AUTO_BINNING_MODE, 0);
    ret |= omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_LL_ALGO, 0);

    return change_config(sensor);
}

static int set_framerate(sensor_t *sensor, int framerate) {
    if (omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_CAM_AET_MAX_FRAME_RATE, framerate * 256) != 0) {
        return -1;
    }

    if (omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_CAM_AET_MIN_FRAME_RATE, framerate * 128) != 0) {
        return -1;
    }

    return change_config(sensor);
}

static int set_contrast(sensor_t *sensor, int level) {
    // -16 to +16
    int new_level = (level > 0) ? (level * 2) : level;

    if ((new_level < -16) || (32 < new_level)) {
        return -1;
    }

    if (omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_UVC_CONTRAST_CONTROL, new_level + 32) != 0) {
        return -1;
    }

    return refresh(sensor);
}

static int set_brightness(sensor_t *sensor, int level) {
    // -16 to +16
    int new_level = level * 2;

    if ((new_level < -32) || (32 < new_level)) {
        return -1;
    }

    if (omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_UVC_BRIGHTNESS_CONTROL, new_level + 55) != 0) {
        return -1;
    }

    return refresh(sensor);
}

static int set_saturation(sensor_t *sensor, int level) {
    // -16 to +16
    int new_level = level * 8;

    if ((new_level < -128) || (128 < new_level)) {
        return -1;
    }

    new_level = IM_MIN(new_level, 127);

    if (omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_UVC_SATURATION_CONTROL, new_level + 128) != 0) {
        return -1;
    }

    return refresh(sensor);
}

static int set_gainceiling(sensor_t *sensor, gainceiling_t gainceiling) {
    return 0;
}

static int set_colorbar(sensor_t *sensor, int enable) {
    if (omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_CAM_MODE_SELECT, enable ? 2 : 0) != 0) {
        return -1;
    }

    return change_config(sensor);
}

static int set_auto_gain(sensor_t *sensor, int enable, float gain_db, float gain_db_ceiling) {
    if (omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_UVC_AE_MODE_CONTROL, enable ? 0x2 : 0x1) != 0) {
        return -1;
    }

    if ((enable == 0) && (!isnanf(gain_db)) && (!isinff(gain_db))) {
        int gain = IM_MAX(IM_MIN(expf((gain_db / 20.0f) * M_LN10) * 32.0f, 0xffff), 0x0000);

        if (omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_UVC_GAIN_CONTROL, gain) != 0) {
            return -1;
        }
    }

    return refresh(sensor);
}

static int get_gain_db(sensor_t *sensor, float *gain_db) {
    uint16_t gain;

    if (omv_i2c_readw2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_UVC_GAIN_CONTROL, &gain) != 0) {
        return -1;
    }

    *gain_db = 20.0f * log10f(gain / 32.0f);

    return 0;
}

static int set_auto_exposure(sensor_t *sensor, int enable, int exposure_us) {
    if (omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_UVC_AE_MODE_CONTROL, enable ? 0x2 : 0x1) != 0) {
        return -1;
    }

    if ((enable == 0) && (exposure_us >= 0)) {
        if (omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_UVC_MANUAL_EXPOSURE_CONFIG, 0x1) != 0) {
            return -1;
        }

        int exposure_100_us = exposure_us / 100;

        if (omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_UVC_EXPOSURE_TIME_ABSOLUTE_CTRL,
                            exposure_100_us >> 16) != 0) {
            return -1;
        }

        if (omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_UVC_EXPOSURE_TIME_ABSOLUTE_CTRL + 2,
                            exposure_100_us) != 0) {
            return -1;
        }
    }

    return refresh(sensor);
}

static int get_exposure_us(sensor_t *sensor, int *exposure_us) {
    uint16_t reg_h, reg_l;

    if (omv_i2c_readw2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_UVC_EXPOSURE_TIME_ABSOLUTE_CTRL, &reg_h) != 0) {
        return -1;
    }

    if (omv_i2c_readw2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_UVC_EXPOSURE_TIME_ABSOLUTE_CTRL + 2, &reg_l) != 0) {
        return -1;
    }

    *exposure_us = ((reg_h << 16) | reg_l) * 100;

    return 0;
}

static int set_auto_whitebal(sensor_t *sensor, int enable, float r_gain_db, float g_gain_db, float b_gain_db) {
    if (omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_UVC_WHITE_BALANCE_AUTO_CONTROL,
                        enable ? 0x1 : 0x0) != 0) {
        return -1;
    }

    return 0;
}

static int get_rgb_gain_db(sensor_t *sensor, float *r_gain_db, float *g_gain_db, float *b_gain_db) {
    *r_gain_db = 0;
    *g_gain_db = 0;
    *b_gain_db = 0;

    return 0;
}

static int set_hmirror(sensor_t *sensor, int enable) {
    uint16_t reg_data;

    if (omv_i2c_readw2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_SENSOR_CONTROL_READ_MODE, &reg_data) != 0) {
        return -1;
    }

    reg_data = (reg_data & (~MT9M114_SENSOR_CONTROL_READ_MODE_HMIRROR)) |
               (enable ? 0x0 : MT9M114_SENSOR_CONTROL_READ_MODE_HMIRROR); // inverted

    if (omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_SENSOR_CONTROL_READ_MODE, reg_data) != 0) {
        return -1;
    }

    return change_config(sensor);
}

static int set_vflip(sensor_t *sensor, int enable) {
    uint16_t reg_data;

    if (omv_i2c_readw2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_SENSOR_CONTROL_READ_MODE, &reg_data) != 0) {
        return -1;
    }

    reg_data = (reg_data & (~MT9M114_SENSOR_CONTROL_READ_MODE_VFLIP)) |
               (enable ? 0x0 : MT9M114_SENSOR_CONTROL_READ_MODE_VFLIP); // inverted

    if (omv_i2c_writew2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_SENSOR_CONTROL_READ_MODE, reg_data) != 0) {
        return -1;
    }

    return change_config(sensor);
}

static int set_special_effect(sensor_t *sensor, sde_t sde) {
    switch (sde) {
        case SDE_NEGATIVE:
            if (omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_CAM_SFX_CONTROL, 0x3) != 0) {
                return -1;
            }
            break;
        case SDE_NORMAL:
            if (omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, MT9M114_REG_CAM_SFX_CONTROL, 0x0) != 0) {
                return -1;
            }
            break;
        default:
            return -1;
    }

    return refresh(sensor);
}

static int ioctl(sensor_t *sensor, int request, va_list ap) {
    int ret = 0;

    switch (request) {
        case IOCTL_SET_READOUT_WINDOW: {
            int tmp_readout_x = va_arg(ap, int);
            int tmp_readout_y = va_arg(ap, int);
            int tmp_readout_w = IM_MAX(IM_MIN(va_arg(ap, int), ACTIVE_SENSOR_WIDTH), resolution[sensor->framesize][0]);
            int tmp_readout_h = IM_MAX(IM_MIN(va_arg(ap, int), ACTIVE_SENSOR_HEIGHT), resolution[sensor->framesize][1]);
            int readout_x_max = (ACTIVE_SENSOR_WIDTH - tmp_readout_w) / 2;
            int readout_y_max = (ACTIVE_SENSOR_HEIGHT - tmp_readout_h) / 2;
            tmp_readout_x = IM_MAX(IM_MIN(tmp_readout_x, readout_x_max), -readout_x_max);
            tmp_readout_y = IM_MAX(IM_MIN(tmp_readout_y, readout_y_max), -readout_y_max);
            bool changed = (tmp_readout_x != readout_x) || (tmp_readout_y != readout_y) ||
                           (tmp_readout_w != readout_w) || (tmp_readout_h != readout_h);
            readout_x = tmp_readout_x;
            readout_y = tmp_readout_y;
            readout_w = tmp_readout_w;
            readout_h = tmp_readout_h;
            if (changed && (sensor->framesize != FRAMESIZE_INVALID)) {
                ret |= set_framesize(sensor, sensor->framesize);
            }
            break;
        }
        case IOCTL_GET_READOUT_WINDOW: {
            *va_arg(ap, int *) = readout_x;
            *va_arg(ap, int *) = readout_y;
            *va_arg(ap, int *) = readout_w;
            *va_arg(ap, int *) = readout_h;
            break;
        }
        default: {
            ret = -1;
            break;
        }
    }

    return ret;
}

int mt9m114_init(sensor_t *sensor) {
    // Initialize sensor structure.
    sensor->reset = reset;
    sensor->sleep = sleep;
    sensor->read_reg = read_reg;
    sensor->write_reg = write_reg;
    sensor->set_pixformat = set_pixformat;
    sensor->set_framesize = set_framesize;
    sensor->set_framerate = set_framerate;
    sensor->set_contrast = set_contrast;
    sensor->set_brightness = set_brightness;
    sensor->set_saturation = set_saturation;
    sensor->set_gainceiling = set_gainceiling;
    sensor->set_colorbar = set_colorbar;
    sensor->set_auto_gain = set_auto_gain;
    sensor->get_gain_db = get_gain_db;
    sensor->set_auto_exposure = set_auto_exposure;
    sensor->get_exposure_us = get_exposure_us;
    sensor->set_auto_whitebal = set_auto_whitebal;
    sensor->get_rgb_gain_db = get_rgb_gain_db;
    sensor->set_hmirror = set_hmirror;
    sensor->set_vflip = set_vflip;
    sensor->set_special_effect = set_special_effect;
    sensor->ioctl = ioctl;

    // Set sensor flags
    sensor->hw_flags.vsync = 0;
    sensor->hw_flags.hsync = 0;
    sensor->hw_flags.pixck = 0;
    sensor->hw_flags.fsync = 0;
    sensor->hw_flags.jpege = 0;
    sensor->hw_flags.gs_bpp = 2;
    sensor->hw_flags.rgb_swap = 0;
    sensor->hw_flags.bayer = SENSOR_HW_FLAGS_BAYER_GBRG;
    sensor->hw_flags.yuv_order = SENSOR_HW_FLAGS_YVU422;

    return 0;
}

#endif // (OMV_ENABLE_MT9M114 == 1)
